using System;
using System.IO;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using FlatRedBall;
using FlatRedBall.Input;
using FlatRedBall.AI.Pathfinding;
using FlatRedBall.Graphics.Animation;
using FlatRedBall.Graphics.Particle;
using FlatRedBall.Math.Geometry;
using FlatRedBall.Math.Splines;
using FlatRedBall.Graphics;
using FlatRedBall.Graphics.Texture;
using System.Collections;

#if FRB_XNA || SILVERLIGHT
using Keys = Microsoft.Xna.Framework.Input.Keys;
using Vector3 = Microsoft.Xna.Framework.Vector3;
using Texture2D = Microsoft.Xna.Framework.Graphics.Texture2D;
#endif

namespace WinRos_Xna.Screens
{
    using WinRos;
	public partial class IrobotCreateScreen
    {
        #region Variables
        Keyboard keyboard = InputManager.Keyboard;
        Xbox360GamePad controller1 = InputManager.Xbox360GamePads[0];
        WinRos ros;
        //make another constructor for win ros
        double timeCalled;
        String service = "";
        //Text text;
        String received = "";
        //int numberOfRays = 4;
        float offset = 2.42f;
        Sprite right;
        Sprite left;
        Sprite cliffRight;
        Sprite cliffLeft;
        SpriteList rayList = new SpriteList();
        Sprite bumper1;
        Random rand;
        float rayLength;
        double origXF = 0.1;
        double origZC = -0.5;
        double origZCC = 0.5;
        double orgiXB = -0.1;
        double x = 0.0;
        double z = 0.0;
        String theTopic = "";
        bool getOut = false;
        Hashtable serviceReturn;
        double rightVal = 100;
        double leftVal = 100;
        double cliffRightVal = 100;
        double cliffLeftVal = 100;
        Text cliffFrontRightText = TextManager.AddText("");
        Text cliffFrontLeftText = TextManager.AddText("");
        Text rightText = TextManager.AddText("");
        Text leftText = TextManager.AddText("");
        bool SHOW_DEBUG_TEXT = true;
        string picService = "";
        string picService1 = "";
        string image = "";
        Sprite video;
        float framerate = 0.0f;
        Text frameText = TextManager.AddText("");
        MemoryStream memStream;
        Screen bumpScreen;
        bool isBump = false;
        #endregion


        public MemoryStream decodeFrom64(string encodedData/*, MemoryStream ms*/)
        {
            byte[] encodedDataAsBytes = System.Convert.FromBase64String(encodedData);

            //ms.Read(encodedDataAsBytes, 0, encodedDataAsBytes.Length);
            MemoryStream ms = new MemoryStream(encodedDataAsBytes, 0, encodedDataAsBytes.Length);

            ms.Write(encodedDataAsBytes, 0, encodedDataAsBytes.Length);

            return ms;
        }

        private void debugText()
        {
            if(SHOW_DEBUG_TEXT)
            {
                rightText.DisplayText = "RightSignal: " + rightVal.ToString();
                rightText.X = -21;
                rightText.Y = 15;
                rightText.Red = 1.0f;
                rightText.Blue = 0.0f;
                rightText.Green = 0.0f;
                rightText.ColorOperation = ColorOperation.ColorTextureAlpha;

                leftText.DisplayText = "LeftSignal: " + leftVal.ToString();
                leftText.X = -21;
                leftText.Y = 14;
                leftText.Red = 0.0f;
                leftText.Blue = 1.0f;
                leftText.Green = 0.0f;
                leftText.ColorOperation = ColorOperation.ColorTextureAlpha;

                cliffFrontRightText.DisplayText = "CliffFrontRightSignal: " + cliffLeftVal.ToString();
                cliffFrontRightText.X = -21;
                cliffFrontRightText.Y = 13;

                cliffFrontLeftText.DisplayText = "CliffFrontLeftSignal: " + cliffLeftVal.ToString();
                cliffFrontLeftText.X = -21;
                cliffFrontLeftText.Y = 12;
                cliffFrontLeftText.Red = 0.0f;
                cliffFrontLeftText.Blue = 0.0f;
                cliffFrontLeftText.Green = 1.0f;
                cliffFrontLeftText.ColorOperation = ColorOperation.ColorTextureAlpha;
           }

        }

        public void getFrameRate()
        {
            framerate = 1 / TimeManager.SecondDifference;

            frameText.DisplayText = "FPS: " + framerate.ToString();
            frameText.X = -22;
            frameText.Y = -15;
            
        }
        public void createBars()
        {
            right = new WinRos_Xna.Entities.Ray(ContentManagerName).rightSig;
            left = new WinRos_Xna.Entities.Ray(ContentManagerName).leftSig;
            cliffRight = new WinRos_Xna.Entities.Ray(ContentManagerName).cliffRightSig;
            cliffLeft = new WinRos_Xna.Entities.Ray(ContentManagerName).cliffLeftSig;

            SpriteManager.AddSprite(right);
            SpriteManager.AddSprite(left);
            SpriteManager.AddSprite(cliffRight);
            SpriteManager.AddSprite(cliffLeft);
                
            //red
            right.Red = 1.0f;
            right.Blue = 0.0f;
            right.Green = 0.0f;

            right.ColorOperation = ColorOperation.Color;

            //blue
            left.Red = 0.0f;
            left.Blue = 1.0f;
            left.Green = 0.0f;

            left.ColorOperation = ColorOperation.Color;

            //green
            cliffLeft.Red = 0.0f;
            cliffLeft.Blue = 0.0f;
            cliffLeft.Green = 1.0f;

            cliffLeft.ColorOperation = ColorOperation.Color;

            right.AttachTo(CreateInstance, true);
            left.AttachTo(CreateInstance, true);
            cliffRight.AttachTo(CreateInstance, true);
            cliffLeft.AttachTo(CreateInstance, true);

            rayList.Add(left);
            rayList.Add(cliffLeft);
            rayList.Add(cliffRight);
            rayList.Add(right);

            rayLength = right.ScaleY;

            Console.WriteLine("The ray list is {0}", rayList.Count);
            
            for (int i = 0; i < rayList.Count; i++)
            {
                rayList[i].RelativeX = -3.65f + (i * offset);
                rayList[i].RelativeY = 6.7f;
            }

        }
        public void CustomInitialize()
		{
            ros = new WinRos(GlobalData.ipaddress); //this makes my neck itch
            //ros = new WinRos("");
            //this where the command service is being called
            service = "{\"receiver\":\"/rosjs/subscribe\",\"msg\":[\"/sensorPacket\",0]}";
            picService = "{\"receiver\":\"/rosjs/subscribe\",\"msg\":[\"/gscam/image_raw\",0]}";
            picService1 = "{\"receiver\":\"/rosjs/subscribe\",\"msg\":[\"/gscam/image_raw\",0,\"jpeg\",1024,1024,50]}";
            ros.publish(service);
            ros.publish(picService1);
            createBars();

            bumper1 = new WinRos_Xna.Entities.Bumper(ContentManagerName).bumperObj;

            SpriteManager.AddSprite(bumper1);
            bumper1.AttachTo(CreateInstance, true);
            bumper1.RelativeY = 4.6f;

            //must be changed
            //GlobalData.controlType = true;

            //CreateInstance.AttachTo(SpriteManager.Cameras[0], true);
            CreateInstance.Position = new Vector3(-15, 0, 0);
            while (getOut == false)
            {
                if (ros.isAvailable() >= 0)
                {
                    serviceReturn = ros.receive(); 
                    //theTopic = ros.receive();
                    getOut = true;
                }
                else
                {
                    //means that there is nothing to receive yet
                }
            }

            video = new Sprite();
            SpriteManager.AddSprite(video);

            memStream = new MemoryStream();
            
           
		}

		public void CustomActivity(bool firstTimeCalled)
		{
            debugText();
            getFrameRate();
            //where the listenin to the socket should be called
            //should block until data is read
            if(ros.isAvailable() >= 0)
            {
                Console.WriteLine("Ready to receice a message");
                serviceReturn= ros.receive();

                try
                {

                    if (((Hashtable)serviceReturn["msg"]).ContainsKey("uri"))
                    {
                        image = Convert.ToString(((Hashtable)serviceReturn["msg"])["uri"]);
                        image = image.Remove(0, 23);//removes the json part of the string

                        MemoryStream stream = decodeFrom64(image);
                        // MemoryStream stream = decodeFrom64(image, memStream);

                        //these next lines return the buffer to the original position
                        stream.Flush();
                        stream.Position = 0L;//zero base64

                        video.Texture = Texture2D.FromFile(FlatRedBallServices.GraphicsDevice, stream);
                        video.ScaleX = 10.5f;
                        video.ScaleY = 10.5f;
                        video.Position = new Vector3(5, 0, 0);


                    }

                    if ((bool)((Hashtable)serviceReturn["msg"])["bumpLeft"]
                        && (bool)((Hashtable)serviceReturn["msg"])["bumpRight"])
                    {
                        //bumper1.ColorOperation = ColorOperation.InterpolateColor;
                        bumper1.Red = 1.0f;
                        bumper1.Blue = .0f;
                        bumper1.Green = 1.0f;
                        bumper1.ColorOperation = ColorOperation.Color;

                        //popup
                        isBump = true;
                        bumpScreen = LoadPopup(typeof(bumpPop).FullName, false);
                        timeCalled = TimeManager.CurrentTime;
                    }
                    else
                    {
                        bumper1.Red = 0.0f;
                        bumper1.Blue = 0.0f;
                        bumper1.Green = 0.0f;
                        bumper1.ColorOperation = ColorOperation.Add;
                    }


                    if (((Hashtable)serviceReturn["msg"]).ContainsKey("cliffRightSignal"))
                    {
                        rightVal = Convert.ToInt32(((Hashtable)serviceReturn["msg"])["cliffRightSignal"]);
                        rightVal = (rightVal * .1);
                        Console.WriteLine(rightVal);
                    }
                    if (((Hashtable)serviceReturn["msg"]).ContainsKey("cliffLeftSignal"))
                    {
                        leftVal = Convert.ToInt32(((Hashtable)serviceReturn["msg"])["cliffLeftSignal"]);
                        leftVal = (leftVal * .1);
                    }

                    if (((Hashtable)serviceReturn["msg"]).ContainsKey("cliffFrontRightSignal"))
                    {
                        cliffRightVal = Convert.ToInt32(((Hashtable)serviceReturn["msg"])["cliffFrontRightSignal"]);
                        cliffRightVal = (cliffRightVal * .1);
                    }

                    if (((Hashtable)serviceReturn["msg"]).ContainsKey("cliffFrontLeftSignal"))
                    {
                        cliffLeftVal = Convert.ToInt32(((Hashtable)serviceReturn["msg"])["cliffFrontLeftSignal"]);
                        cliffLeftVal = (cliffLeftVal * .1);
                    }
                }
                catch (InvalidCastException)
                {
                    //do nothing
                    Console.WriteLine("Can't read info from socket");
                }
                catch (NullReferenceException)
                {
                    //do nothing
                    Console.WriteLine("Cant read info from socket");
                }

                //Get rid of the pop up
                if ((TimeManager.CurrentTime - timeCalled > 1) && isBump )
                {
                    bumpScreen.IsActivityFinished = true;
                    
                }

                //right
                right.ScaleY = (float)(rayLength * (rightVal / 100f));
                right.RelativeY = (float)((rayLength * (rightVal / 100f)) + (rayLength * 3) - .1f);
                //right.ColorOperation = ColorOperation.Add;

                //left
                left.ScaleY = (float)(rayLength * (leftVal / 100f));
                left.RelativeY = (float)((rayLength * (leftVal / 100f)) + (rayLength * 3) - .1f);

                //cliff right
                cliffRight.ScaleY = (float)(rayLength * (cliffRightVal / 100f));
                cliffRight.RelativeY = (float)((rayLength * (cliffRightVal / 100f)) + (rayLength * 3) - .1f);

                //cliff left
                cliffLeft.ScaleY = (float)(rayLength * (cliffLeftVal / 100f));
                cliffLeft.RelativeY = (float)((rayLength * (cliffLeftVal / 100f)) + (rayLength * 3) - .1f);

            }
            if (GlobalData.controlType)
            {
                //Console.WriteLine("Got to the input data stage");
                if(keyboard.KeyDown(Keys.Escape) || controller1.ButtonDown(Xbox360GamePad.Button.Back))
                {
                    //exit the game but doesnt work yet
                    //ros.disconnect();
                    //Environment.Exit(0);
                    FlatRedBallServices.Game.Exit();
                }

                else if (keyboard.KeyDown(Keys.Up) || controller1.ButtonDown(Xbox360GamePad.Button.A))
                {
                    Console.WriteLine("up movement");
                    x = origXF;
                    z = 0.0;
                    String theMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + x + ",\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":" + z + "}},\"type\":\"geometry_msgs/Twist\"}";
                    
                    //ros stuff
                    ros.publish(theMessage); //publish the message
                }

                else if (keyboard.KeyDown(Keys.Down) || controller1.ButtonDown(Xbox360GamePad.Button.Y))
                {
                    Console.WriteLine("down movement");
                    x = orgiXB;
                    z = 0.0;
                    String theMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + x + ",\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":" + z + "}},\"type\":\"geometry_msgs/Twist\"}";
                    
                    //ros stuff
                    ros.publish(theMessage);

                    
                }
                else if (keyboard.KeyDown(Keys.A) || controller1.ButtonDown(Xbox360GamePad.Button.LeftTrigger))
                {
                    z = origZCC;
                    String theMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + x + ",\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":" + z + "}},\"type\":\"geometry_msgs/Twist\"}";
                                     
                    //ros stuff
                    ros.publish(theMessage);

                }
                else if (keyboard.KeyDown(Keys.D) || controller1.ButtonDown(Xbox360GamePad.Button.RightTrigger))
                {
                    z = origZC;
                    String theMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + x + ",\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":" + z + "}},\"type\":\"geometry_msgs/Twist\"}";
                    //ros stuff
                    ros.publish(theMessage);
                }
                else if (keyboard.KeyDown(Keys.J) || controller1.ButtonPushed(Xbox360GamePad.Button.X))
                {
                    //increase linear speed
                    origXF *= 1.10;
                    orgiXB *= 1.10;
                }

                else if (keyboard.KeyDown(Keys.L) || controller1.ButtonPushed(Xbox360GamePad.Button.B))
                {
                    //decrease linear speed
                    orgiXB *= 0.9;
                    origXF *= 0.9;
                }
                else if (keyboard.KeyDown(Keys.I) || controller1.ButtonPushed(Xbox360GamePad.Button.LeftShoulder))
                {
                    //increase angular speed
                    origZC *= 1.10;
                    origZCC *= 1.10;
                }
                else if (keyboard.KeyDown(Keys.O) || controller1.ButtonPushed(Xbox360GamePad.Button.RightShoulder))
                {
                    //decrease angular speed
                    origZC *= 0.9;
                    origZCC *= 0.9;
                }
                else
                {
                    CreateInstance.Velocity = new Vector3(0, 0, 0);
                    x = 0.0;
                    z = 0.0;
                    String theMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + x + ",\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":" + z + "}},\"type\":\"geometry_msgs/Twist\"}";
                    //ros stuff
                    ros.publish(theMessage);
                }
            }

            else
            {
                //control scheme two unsupported as of now
            }
             
		}

		public void CustomDestroy()
		{
            if (SHOW_DEBUG_TEXT)
            {
                TextManager.RemoveText(rightText);
                TextManager.RemoveText(leftText);
                TextManager.RemoveText(cliffFrontLeftText);
                TextManager.RemoveText(cliffFrontRightText);
            }

            TextManager.RemoveText(frameText);
            mPopups.Remove(bumpScreen);
            bumpScreen.Destroy();
		}

        private static void CustomLoadStaticContent(string contentManagerName)
        {


        }

	}
}
